package com.pudu.pudusclient;

import android.util.Log;

import com.pudu.mydemo.view.rplidar_node_t;
import com.pudu.pdrobot.App;

import java.util.ArrayList;
import java.util.List;

/**
 * Created by fanyp on 2016/9/12.
 */
public class HLSRadarData {
    static HLSRadarData mInstance;

    private HLSRadarData()
    {
    }

    public static HLSRadarData getInstance(){
        if( mInstance == null)
        {
            mInstance = new  HLSRadarData();
        }
        return mInstance;
    }

    public final static float PI = (float) 3.14159265;

    public List<rplidar_node_t> ParserData(byte[] newReceiveData)
    {
        List<rplidar_node_t> nodelist = new ArrayList<rplidar_node_t>();

        if (newReceiveData.length != 42) {
            Log.i("fffggg", "radar error:" + newReceiveData.length);
            return nodelist;
        } else {
            Log.i("fffggg", "radar info:" + newReceiveData.length);
        }

        if(App.getInstance().mHlsVersion == 1) {
            int AngleIndex = unsignedByteToInt(newReceiveData[2]);
            int data = unsignedByteToInt(newReceiveData[4]);
            data = (int)(data << 8);
            int RPM = unsignedByteToInt(newReceiveData[3]) + data;

            int nAngle = (AngleIndex - 160) * 6;

            for(int i = 0; i < 6; i ++)
            {
                rplidar_node_t obj = new rplidar_node_t();
                obj.angle = nAngle + i;

                int value = unsignedByteToInt(newReceiveData[8 + 6*i]);
                value = (int)(value << 8);
                obj.dist = unsignedByteToInt(newReceiveData[7 + 6*i]) + value;

                float distPixel = obj.dist;
                float rad = (float) (obj.angle * PI / 180.0);
                obj.ptX = (float) (Math.sin(rad) * (distPixel));
                obj.ptY = (float) (Math.cos(rad) * (distPixel));

                nodelist.add(obj);
//                Log.i("aaacccc", "angle:" + obj.angle + "dist:" + obj.dist);
            }
        }else {
            int AngleIndex = unsignedByteToInt(newReceiveData[1]);
            int data = unsignedByteToInt(newReceiveData[3]);
            data = (int) (data << 8);
            int RPM = unsignedByteToInt(newReceiveData[2]) + data;

            int nAngle = (AngleIndex - 160) * 6;

            for (int i = 0; i < 6; i++) {
                rplidar_node_t obj = new rplidar_node_t();
                obj.angle = nAngle + i;

                int value = unsignedByteToInt(newReceiveData[7 + 6 * i]);
                value = (int) (value << 8);
                obj.dist = unsignedByteToInt(newReceiveData[6 + 6 * i]) + value;

                float distPixel = obj.dist;
                float rad = (float) (obj.angle * PI / 180.0);
                obj.ptX = (float) (Math.sin(rad) * (distPixel));
                obj.ptY = (float) (Math.cos(rad) * (distPixel));

                nodelist.add(obj);
//                Log.i("aaacccc", "angle:" + obj.angle + "dist:" + obj.dist);
            }
        }

        return nodelist;
    }

    public static int Distance_In_Front_Obstacle = 0;

    public void Isolation(List<rplidar_node_t> lsDatas){

        int nMinLength = -1;
        rplidar_node_t nodeMin = null;
        for(int i = 0; i < lsDatas.size(); i ++)
        {
            rplidar_node_t node = lsDatas.get(i);

            if(120 <= node.angle && 240 > node.angle) {
                if(Math.abs(node.ptX) <= 310)
                {
                    int dist =  (int) Math.abs(node.dist);
                    if(dist<20) {
                        continue;
                    }
                    if( nMinLength == -1) {
                        nMinLength = dist;
                        nodeMin = node;
                        continue;
                    }else if(nMinLength > dist){
                        nMinLength = dist;
                        nodeMin = node;
                        continue;
                    }
                }
            }
        }

        if(nMinLength == -1) {
            Distance_In_Front_Obstacle = -1;
        }else{
            Distance_In_Front_Obstacle = nMinLength;
        }
        if(nodeMin !=null) {
            Log.i("uuuuu", "Distance_In_Front_Obstacle:" + Distance_In_Front_Obstacle + "Angle:" + nodeMin.angle);
        }else{
            Log.i("uuuuu", "Distance_In_Front_Obstacle:" + Distance_In_Front_Obstacle);
        }
    }


    private static String byte2hex(byte [] buffer){
        String h = "";

        for(int i = 0; i < buffer.length; i++){
            String temp = Integer.toHexString(buffer[i] & 0xFF);
            if(temp.length() == 1){
                temp = "0" + temp;
            }
            h = h + " "+ temp;
        }
        return h;
    }

    public static int unsignedByteToInt(byte b) {
        return (int) b & 0xFF;
    }
}
